import random
import numpy as np
import pygame
import os
import sys
from math import *
import cv2

img=cv2.imread('aaa.png') #背景图片
imgskin=cv2.resize(img,(600,400))


def W2O(xw,yw): #世界坐标,以毫米为单位，在以X轴与朝上的Y轴建立的直角坐标系上
    xs=int(xw*(578-22)/600+22)
    ys=int(yw*(42-358)/400+358)
    return (xs,ys)


#测试世界坐标的合理性，在桌面四个角画园，颜色不同。以毫米为单位，(0,0)左下角为红色，（60，0）右下角为蓝色 ,.....
def W2Otest():
    #1调用W2O(0,0)得到opencv像素点，画红色园
    xs0,ys0=W2O(0,0)
    cv2.circle(imgskin,(xs0,ys0),5,(0,0,255),-1)

    #2
    xs1,ys1=W2O(600,0)
    cv2.circle(imgskin,(xs1,ys1),5,(255,0,0),-1)

    #3
    xs2,ys2=W2O(0,400)
    cv2.circle(imgskin,(xs2,ys2),5,(0,255,0),-1)

    #4
    xs3,ys3=W2O(600,400)
    cv2.circle(imgskin,(xs3,ys3),5,(0,0,0),-1)
    
    
#W2Otest()

def Wline(p1,p2,color=(0,0,0)):
    xw0,yw0=p1
    xw1,yw1=p2
    xs0,ys0=W2O(xw0,yw0)
    xs1,ys1=W2O(xw1,yw1)
    cv2.line(imgskin,(xs0,ys0),(xs1,ys1),color,1)



def Wlinetest():
    Wline((100,100),(300,300))
    Wline((400,100),(350,130))
    Wline((500,300),(550,200))
    Wline((200,200),(250,300))
  
#Wlinetest()

def Wvector(v1,color=(0,0,0)):
    wp1,wp2=v1
    Wline(wp1,wp2,(0,0,0))
    xs,ys=W2O(wp2[0],wp2[1])
    cv2.circle(imgskin,(xs,ys),2,color,-1)

def Wvectortest():
    Wvector(((100,100),(300,300)))
    Wvector(((400,100),(350,130)))
    Wvector(((500,300),(550,200)))
    Wvector(((200,200),(250,300)))

def Vmwdxdy(vin,deltax,deltay): #平移计算
    wp1,wp2=vin
    xs1,ys1=wp1[0],wp1[1]
    xs2,ys2=wp2[0],wp2[1]
     
    
    xs1out=xs1+deltax
    ys1out=ys1+deltay
    xs2out=xs2+deltax
    ys2out=ys2+deltay

    return ((xs1out,ys1out),(xs2out,ys2out))

def lenline(vin): #向量长度
    wp1,wp2=vin
    xs1,ys1=wp1[0],wp1[1]
    xs2,ys2=wp2[0],wp2[1]
    len=((xs2-xs1)**2+(ys2-ys1)**2)**0.5

    return len
    

def Vmxy(vin,x,y):  #顺向量轴向与法向方向平移
    wp1,wp2=vin
    xs1,ys1=wp1[0],wp1[1]
    xs2,ys2=wp2[0],wp2[1]
    if xs2==xs1 and ys2>ys1:
        A=pi/2
    elif xs2==xs1 and ys2<ys1:
        A=3*pi/2
    else:
        A=atan((ys2-ys1)/(xs2-xs1))  
    
    deltax1=x*sin(A)
    deltax2=y*cos(A)
    deltay1=x*-cos(A)
    deltay2=y*sin(A)
    
    return (Vmwdxdy(vin,deltax1+deltax2,deltay1+deltay2)) 

def Vrotate(vin,origin,theta):  #沿一个点逆时针旋转 origin0-1确定旋转中心 弧度输入
    wp1,wp2=vin
    xs1,ys1=wp1[0],wp1[1]
    xs2,ys2=wp2[0],wp2[1]
    if xs2==xs1 and ys2>ys1:
        A=pi/2
    elif xs2==xs1 and ys2<ys1:
        A=3*pi/2
    else:
        A=atan((ys2-ys1)/(xs2-xs1)) 
    
    vinlen=lenline(vin)
    xs3=xs1*(1-origin)+xs2*origin
    ys3=ys1*(1-origin)+ys2*origin
    
    xs4=xs3+cos(A+theta)*vinlen*(1-origin)
    ys4=ys3+sin(A+theta)*vinlen*(1-origin)
    xs5=xs3-cos(A+theta)*vinlen*origin
    ys5=ys3-sin(A+theta)*vinlen*origin

    return ((xs5,ys5),(xs4,ys4))

def Vrotatetest():
    vin=((200,200),(250,300))
    for i in range(12):
        Wvector(Vrotate(vin,0.5,-pi/6*i))
        i=i+1


#Vrotatetest()


def Vf(vin,f1,f2,μ,m,t): #输入为：f1为上方向右的力,f2为下方向左的力，μ为摩擦系数，m为质量(kg) 
    wp1,wp2=vin
    xs1,ys1=wp1[0],wp1[1]
    xs2,ys2=wp2[0],wp2[1]
    g=9.8
    vinlen=lenline(vin)
    if xs2==xs1 and ys2>ys1:
        A=pi/2
    elif xs2==xs1 and ys2<ys1:
        A=3*pi/2
    else:
        A=atan((ys2-ys1)/(xs2-xs1))
    
    f=μ*m*g
    if f1>0 and f2>0:
        ff1=f1-f       
        ff2=f2-f
        frotate=min(ff1,ff2)
    elif f1<0 and f2<0:
        ff1=f1+f       
        ff2=f2+f
        frotate=-min(abs(ff1),abs(ff2))
    elif f1<0 and f2>0:
        ff1=f1+f       
        ff2=f2-f
        frotate=-abs(abs(ff1)-abs(ff2))
    elif f1>0 and f2<0:
        ff1=f1-f
        ff2=f2+f
        frotate=abs(abs(ff1)-abs(ff2))
    fmove=ff1-ff2
    
    
    
    

    
    
    xm=0.5*xs1+0.5*xs2     #旋转点坐标
    ym=0.5*ys1+0.5*ys2

    Theta=-(frotate*t**2/(vinlen*m))
    vrotate=Vrotate(vin,0.5,Theta)
    deltax2=(0.5*fmove*t**2/m)
    vmove=Vmwdxdy(vrotate,deltax2,0)
    return vmove 

   
        

def Vftest():
    vin=((200,200),(200,300))
    for i in range(20): #顺时针只转f1=10,f2=10  逆时针只转f1=-10,f2=-10 左平移f1=-10,f2=10 右平移f1=10,f2=-10
        Wvector(Vf(vin,10,10,0.5,1,0.5*i),(0,0,100+5*i))
        i=i+1

Vftest()




vin=((200,200),(250,300))
Vout=Vf(vin,10,20,0.5,1,5)
print(Vout)
#Wvector(vin)
#Wvector(Vout,(0,0,255))





   










cv2.imshow("hello",imgskin)
cv2.waitKey(1000000)
print("hello")
 